/***************************************************************************
 创建者: 华磊
 开始时间: 2019.7.4
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file .h
 *  功能: 反光条、反光柱定位算法
 * 正常用反光柱定位，反光柱定位失败时，使用电机里程计，电机里程计超过允许长度或角度时，要暂停，反光柱定位恢复后，继续运行。
 *
 *                                                                         *
 ***************************************************************************/

#ifndef REFLECTORMATCH_H
#define REFLECTORMATCH_H
#include <QString>
#include <QMap>
#include "globalDefine.h"
#include "GeneralDefine.h"
#include <QMutex>
#include <vector>


class FilteringAlgorithm;
class DomParser;
using namespace std;
namespace KDL{
class Frame;
}

struct CrossPointInfo
{
    double r1;
    double r2;
    double angle;
    VehiclePosition pose;
};

struct CompareLengthInfo
{
    int source1Index;
    int source2Index;
    double distance;
};

struct LengthErrorInfo
{
    int line1Index;
    int line1StartIndex;
    int line1EndIndex;
    int line2Index;
    int line2StartIndex;
    int line2EndIndex;
    double distanceError;
};

struct PointMapInfo
{
    int laserPointIndex;
    int referencePointIndex;
};



struct PointMatchInfo
{
    LaserMarkInfo laserPoint;
    ReflectMarkInfo referencePoint;
    double matchDistanceError;
};

struct ValidPointCheck
{
    double distance;
    int minCount;
};

class KalmanFilterSimple;

class ReflectorMatch
{
public:
    ReflectorMatch(QString configFileIn, int robotIdIn,int laserSkipCountIn);
public:

    int getReflectLaserFilterParameter(int index,struct LaserFilterParameter &paraOut);
    int setReflectLaserFilterParameter(int index,struct LaserFilterParameter  paraIn);
    int getReflectCalculateCommonParameter(ReflectCommonDetectPara &paraOut);
    int setReflectCalculateCommonParameter( ReflectCommonDetectPara paraIn);

    int notifyAmclPositionInfo(PositionInfo vehiclePositionIn);
    int getAllReferenceMarks(std::vector<ReflectMarkInfo> &reflectMarksOut);
    int addReferenceMarkToMap(ReflectMarkInfo markIn);
    int saveReferenceMarkToFile(QString filePathIn);
    int deleteReferenceMarkInMap(int uniqId);
    int getReflectPositionCertainity(VehiclePosition &uncertainPoseOut);//算法用于暂停和报错。
    int setLaserIntensityFilter(double minumIntensityIn, double maxLaserRangeIn, double minAngleIn, double maxAngleIn);
    int getDebugInfo(ReflectDebugInfo &matchDebugInfoOut);
    int laserFilter_test(const LaserDataInfo &frontLaserDataIn, LaserDataInfo &frontLaserDataOut);
    int setWheelOdometerData_noRos( struct OdemeterData odemeterDataIn);
    int setInitialVehiclePosition(VehiclePosition agvPoseIn);
    int setDebugFlag(int flagIn);
    int loadReflectMapFile(QString filePathIn);
    int setLaserIntensityFilterByType(int typeIn);
    int getTimeConsumed(double &minConsumedOut,double &currentConsumedOut,double &maxConsumedOut);
    int resetRosMeasureTime();
    int setLaserSensorData(const LaserDataInfo *laserData);
    int detectReflectMark_filter(PositionInfo &vehiclePositionInfoOut);
    int detectReflectMark(PositionInfo &vehiclePositionInfoOut);
    /**
     * @brief setLaserLinkInBaseLink
     * @param x unit is m
     * @param y
     * @param z
     * @param roll unit is rad
     * @param pitch
     * @param yaw
     * @return
     */
    int setLaserLinkInBaseLink(double x,double y,double z,double roll,double pitch,double yaw);


private:

    int checkCrossPoints(QVector<CrossPointInfo> &tmpCrossInfoList,
                         int maxAllowMatchNum, double idealRadius, int idealMatchNum );
    bool isCrossAngleOk(double allowMinAngleIn, struct circle_t circlesIn[], struct point_t pointsIn, double &angleOut);
    int calculateAgvPositionProbolityError(VehiclePosition oldPoseIn,VehiclePosition newPoseIn,
                                           VehiclePosition &poseErrorInfoOut);//计算AGV可能误差。与里程和时间关联，校准后复位。
    int laserFilter_reflect(const LaserDataInfo &laserDataIn,LaserDataInfo &laserDataOut);
    int getReferenceMarks(QVector<ReflectMarkInfo> &marksOut);
    int getQualifiedLaserMarkPoints(KDL::Frame *agvInMapFrame, const LaserDataInfo &laserDataIn,
                                    ReflectMarkInfo markInfo, QVector<LaserMarkInfo> &laserMarkOut);
    int calculateMatchPoints(const QVector<ReflectMarkInfo> &referenceMarks, const QVector<LaserMarkInfo> &laserPoints,
                       QVector<PointMatchInfo> &matchPointOut);
    int trasferToMapCoordinate(KDL::Frame *agvInMapFrame, LaserMarkInfo &pointOut);
    int calculateLaserFramePose(const QVector<PointMatchInfo> &matchInfo, KDL::Frame *frame_laser,
                                int &validCrossPointCountOut);

    int comparePoints(const QVector<ReflectMarkInfo> &referenceMarksIn,const QVector<VehiclePosition> &laserMarksIn
                      ,KDL::Frame *frame_laser_to_referenceMark);//not used
    int calculatePointMap(QVector<QVector<LengthErrorInfo> > &lengErrorsIn, QVector<PointMapInfo> &mapInfoOut);
//    int calculateLaserTransferMatrix();
    int calculateAgvPosition_byReflect(KDL::Frame *frame_laselink_in_map, VehiclePosition &baselinkPoseOut);
    int calculateAgvPosition_byOdom(PositionInfo &vehiclePositionInfoOut);

    int calculateCorrectPosition(PositionInfo amclPositionIn, PositionInfo &reflectPositionInOut);

    int calculateOffsetMatrix(VehiclePosition agvInMapPositionIn, KDL::Frame *frame_baselink_in_odomIn,
                              KDL::Frame *frame_odom_in_mapOut);

    int readConfigFile(QString filePathIn, QString &readComent);
    int writeLaserFilterConfigFile(QString filePathIn,QMap<int,LaserFilterParameter> laserFilter_ParameterIn);
    int writeCommonCalculateConfigFile(QString filePathIn,ReflectCommonDetectPara detectParaIn);
    double angleRotationJudgement(double targetAngle, double initialAngle);
    int crossPointFiltering(const QVector<CrossPointInfo> &crossPoints, VehiclePosition& pointOut);
    void addMsg(int messageLevel, std::string componentName, std::string messageType
                , int messageCode, int robotIdIn, double parameter1=0, double parameter2=0,
                int parameter3=0, int parameter4=0, QString comment="");

    int saveOneMarkToXmlFile(DomParser *domParser,ReflectMarkInfo markInfo);
    int initial_MapFile(QString filePathIn);
    bool isPlaneLaserMark(const LaserDataInfo &laserDataIn, int tmpValidCount,
                          int lastInValidIndex, LaserMarkInfo &markPointOut);
    bool isColumnLaserMark(const LaserDataInfo &laserDataIn, int tmpValidCount,
                          int lastInValidIndex, LaserMarkInfo &markPointOut);
//    int filterPositionResult(double &pose_x,double &pose_y,double &pose_r);
    int calculateLaserLinkPosition(VehiclePosition agvInMapPositionIn,VehiclePosition &laserPositionOut);

private:
    QString configFile;
    LaserDataInfo laserData_received;
    bool laserDateReadyCount;
    QMutex usingMutex;
    QMap<int,ReflectMarkInfo> reflectMarks;
    QMap<int,double> planeMarkWidth;
    QMap<int,double> columnMarkDiameter;
    double currentPlaneMarkWidth;//需要根据宽度进行过滤　ｔｏｄｏ
    double currentColumnMarkDiameter;//需要根据直径来过滤　ｔｏｄｏ
    QMap<int,LaserFilterParameter> laserFilter_Parameter;

    int robotId;
    int laserSkipCount;

    double minConsumed;
    double currentConsumed;
    double maxConsumed;

    KDL::Frame *frame_baselink_in_laserlink;
    KDL::Frame *frame_laserlink_in_baselink;

    OdemeterData odemeterData;//里程计信息
    KDL::Frame *frame_baselink_in_odom;//轮子里程计
    KDL::Frame *frame_odom_in_map;//里程计偏差
    double odomOffset_x;
    double odomOffset_y;
    double odomOffset_r;

    PositionInfo agvInMapPosition;//初始位置，校正位置
    PositionInfo agvInMapPosition_old;
    VehiclePosition laserlinkInMapPosition;//激光雷达初始位置，校正位置
//    KDL::Frame *agvInMapFrame;//初始位置，校正位置
    VehiclePosition poseErrorInfo;//位置误差

    int debugFlag;
    bool isInitialOkStatus;
    bool isMatchSucess;
    int currentLaserFilterType;
    LaserFilterParameter currentLaserFilter;

    //一般参数
    ReflectCommonDetectPara detectPara;
//    double allowMinMarkDistance;
//    double minimumReferencePointCount;//最小参考点数
//    double maxmumReferencePointCount;//最多参考点数
//    double maxMatchLineDistanceOffset;//not used
//    double initial_AgvPositionOffset;//初始定位时　允许匹配时位置误差
//    double initial_AgvAngleOffset;//初始定位时　允许匹配时角度误差
//    double mached_AgvPositionOffset;//匹配后　允许匹配时位置误差
//    double mached_AgvAngleOffset;//匹配后　允许匹配时角度误差
//    double odomErrorRatio_position;//里程计距离误差比例系数
//    double odomErrorRatio_angle;//里程计角度误差比例系数
//    double laserIntensityLoseRatio;//雷达反射光强随着距离衰减的比例系数
//    double matchColumnMarkDiameterTolorenceRatio;//圆柱标记识别，允许的直径偏差比例
//    double matchPlaneMarkMinWidth;//匹配平面标记时，允许最小的宽度
//    double matchPlaneMarkMaxWidth;//匹配平面标记时，允许最大的宽度
//    double odomOffsetFilterCount;//里程计偏差滤波数
//    double minCorssAngle;
//    bool isCorrectByAmcl;
//    double qualifyAmclMinCertaintyScore;
//    int correctFilterCount;
//    double correctAllowMaxDistanceOffset;
//    double correctAllowMaxAngleOffset;
//    double correctOdomErrorDistanceThreshold;
//    double correctOdomErrorAngleThreshold;


    ReflectDebugInfo matchDebugInfo;
    double laser_roll,laser_pitch,laser_yaw;
    FilteringAlgorithm* filtering_position_x;
    FilteringAlgorithm* filtering_position_y;
    FilteringAlgorithm* filtering_position_r;
    KalmanFilterSimple *kalmanFilter;
    QVector<ValidPointCheck> validPointCheckList;
    int initialPositionCount;
    PositionInfo amclPosition;
};

#endif // REFLECTORMATCH_H
